package Basic;

import java.util.Vector;

import org.rtscup.core.Obstacle;

public class TilesAntigo {
	
	private int width;
	private int height;
	private int noTiles;
	
	private boolean[] obstacles;
	
	public TilesAntigo(int width, int height, int noTiles, Vector<Obstacle> obstacles){
		this.width = width;
		this.height = height;
		this.noTiles = noTiles;
		this.obstacles = new boolean[(int)Math.pow(this.noTiles,2)];
		for(int i = 0 ; i < obstacles.size() ; i++){
			Obstacle o = obstacles.get(i);
			int tile1 = this.getTile(new Point(o.getX()-(o.getWidth()/2),o.getY()-(o.getHeight()/2)));
			int tile2 = this.getTile(new Point(o.getX()+(o.getWidth()/2),o.getY()-(o.getHeight()/2)));
			int tile3 = this.getTile(new Point(o.getX()-(o.getWidth()/2),o.getY()+(o.getHeight()/2)));
			//System.out.println(tile1+" "+tile2+" "+tile3);
			int rangeW = tile2-tile1+1;
			int rangeH = ((tile3-tile1)/this.noTiles)+1;
			//System.out.println(rangeW+" "+rangeH);
			for(int m = 0 ; m < rangeH ; m++){
				for(int n = 0 ; n < rangeW ; n++){
					//System.out.println(n+(m*this.noTiles)+tile1);
					this.obstacles[n+(m*this.noTiles)+tile1] = true;
				}
			}
		}
		/*
		System.out.println("Obstacles:");
		for(int i = 0 ; i < this.obstacles.length ; i++){
			System.out.println(i+" -> "+this.obstacles[i]);
		} System.out.println();
		*/
		/*
		System.out.println();
		System.out.println("Obstacles:");
		for(int i = 0 ; i < this.obstacles.length ; i++){
			if(this.obstacles[i])System.out.println(i);
		} System.out.println();
		System.out.println();
		*/
	}
	
	public Point getCentral(Point p){
		return this.getCentral(this.getTile(p));
	}
	
	public Point getCentral(int tile){
		int w = tile%this.noTiles;
		int h = (tile - w)/this.noTiles;
		double wm = (double)w+0.5;
		double hm = (double)h+0.5;
		Point ret = new Point((int)((wm/(double)this.noTiles)*this.width),
				(int)((hm/(double)this.noTiles)*this.height));
		return ret;
	}
	
	public int getTile(Point p){
		int x = 0; int y = 0;
		if(p.getX()>=this.width) x = this.width-1; else x = p.getX();
		if(p.getY()>=this.height) y = this.height-1; else y = p.getY();
		int w = x/(this.width/this.noTiles);
		int h = y/(this.height/this.noTiles);
		return (w*1+h*this.noTiles);
	}
	
	public Vector<Integer> getNeighborsTiles(int tile){
		
		Vector<Integer> ret = new Vector<Integer>();
		
		if(tile/this.noTiles >= 1){
			if(!this.obstacles[tile-(this.noTiles)])ret.add(tile-(this.noTiles));
			if(tile%this.noTiles != 0 && !this.obstacles[tile-(this.noTiles+1)]) ret.add(tile-(this.noTiles+1));
			if((tile+1)%this.noTiles != 0 && !this.obstacles[tile-(this.noTiles-1)]) ret.add(tile-(this.noTiles-1));  
		}
		
		if(tile%this.noTiles != 0 && !this.obstacles[tile-1]) ret.add(tile-1);
		if((tile+1)%this.noTiles != 0  && !this.obstacles[tile+1]) ret.add(tile+1);
		
		if(tile/this.noTiles < (this.noTiles-1)){
			if(!this.obstacles[tile+(this.noTiles)])ret.add(tile+(this.noTiles));
			if(tile%this.noTiles != 0  && !this.obstacles[tile+(this.noTiles-1)]) ret.add(tile+(this.noTiles-1));
			if((tile+1)%this.noTiles != 0  && !this.obstacles[tile+(this.noTiles+1)]) ret.add(tile+(this.noTiles+1));
		}
		
		return ret;
	}
	
	public int getTotalTiles(){
		return (int)Math.pow(this.noTiles,2);
	}
	
	public Vector<Point> aStar(Point start,Point goal){
		return this.aStar(this.getTile(start),this.getTile(goal));
	}
	
	public Vector<Point> aStar(int start, int goal){
		
		Vector<Integer> closedSet = new Vector<Integer>();
		Vector<Integer> openSet = new Vector<Integer>();
		openSet.add(start);
		
		double[] g = new double[(int)Math.pow(this.noTiles,2)];
		double[] h = new double[(int)Math.pow(this.noTiles,2)];
		double[] f = new double[(int)Math.pow(this.noTiles,2)];
		int[] cameFrom = new int[(int)Math.pow(this.noTiles,2)];
		for(int i = 0 ; i < cameFrom.length ; i++){cameFrom[i]=-1;}
		
		g[start] = 0;
		h[start] = this.euclidean(this.getCentral(start),this.getCentral(goal));
		f[start] = h[start];
		
		while(!openSet.isEmpty()){
			
			int x = 0;
			int index = 0;
			double min = 9999999;
			for(int i = 0 ; i < openSet.size() ; i++){
				if(f[openSet.get(i)] < min){
					x = openSet.get(i);
					min = f[openSet.get(i)];
					index = i;
				}
			}
			if(x==goal) break;
			
			openSet.remove(index);
			closedSet.add(x);
			
			Vector<Integer> neighbors = this.getNeighborsTiles(x);
			for(int i = 0 ; i < neighbors.size() ; i++){
				
				int y = neighbors.get(i);
				
				boolean inClosedSet = false;
				for(int j = 0 ; j < closedSet.size() ; j++){
					if(closedSet.get(j)==y){
						inClosedSet = true;
						break;
					}
				}
				
				if(!inClosedSet){
					
					double tentativeG = g[x]++;
					
					boolean inOpenSet = false;
					for(int j = 0 ; j < openSet.size() ; j++){
						if(openSet.get(j)==y){
							inOpenSet = true;
							break;
						}
					}
					
					boolean tentativeIsBetter = false;
					
					if(!inOpenSet){
						openSet.add(y);
						tentativeIsBetter = true;
					}
					else if(tentativeG < g[y]){
						tentativeIsBetter = true;
					}
					
					if(tentativeIsBetter){
						cameFrom[y] = x;
						g[y] = tentativeG;
						h[y] = this.euclidean(this.getCentral(y),this.getCentral(goal));
						f[y] = g[y] + h[y];
					}
					
				}
				
			}
			
		}
		/*
		System.out.println("Print cameFrom");
		for(int p = 0 ; p < cameFrom.length ; p++){
			System.out.println(p+" -> "+cameFrom[p]);
		} System.out.println();
		*/
		/*
		int iter = goal;
		while(cameFrom[iter] != start) iter = cameFrom[iter];
		
		Point ret = this.getCentral(iter);
		*/
		
		Vector<Point> ret = new Vector<Point>();
		int iter = goal;
		while(iter != start){
			iter = cameFrom[iter];
			ret.add(this.getCentral(iter));
		}
		
		for(int i = 0 ; i < ret.size()/2 ; i++){
			Point temp = ret.get(i);
			ret.set(i,ret.get(ret.size()-i-1));
			ret.set(ret.size()-i-1,temp);
		}
		
		return ret;
	}
	
	public double euclidean(Point p1, Point p2){
		return Math.sqrt(Math.pow((p1.getX()-p2.getX()),2) + Math.pow((p1.getY()-p2.getY()),2));
	}
	
	/* Testing the obstacles 
	public static void main(String[] args) {
		
		Obstacle o1 = new Obstacle("1",0);
		o1.setX(720); o1.setY(120); o1.setWidth(160); o1.setHeight(120);
		Obstacle o2 = new Obstacle("2",1);
		o2.setX(400); o2.setY(320); o2.setWidth(240); o2.setHeight(210);
		Obstacle o3 = new Obstacle("3",2);
		o3.setX(280); o3.setY(510); o3.setWidth(320); o3.setHeight(30);
		
		Vector<Obstacle> vo = new Vector<Obstacle>();
		vo.add(o1); vo.add(o2); vo.add(o3);
		
		Tiles t = new Tiles(800,600,10,vo);
		Point start = new Point(10,10);
		Point goal = new Point(600,450);
		Vector<Point> p = t.aStar(t.getTile(start),t.getTile(goal));
		for(int i = 0 ; i < p.size() ; i++){
			System.out.println((i+1)+" -> "+p.get(i).getX()+","+p.get(i).getY()+" - tile = "+t.getTile(p.get(i)));
		} System.out.println();
		
	}
	*/
	
	/* Testing the A 
	public static void main(String[] args) {
		
		Tiles t = new Tiles(800,600,100);
		Point start = new Point(10,10);
		Point goal = new Point(300,300);
		Vector<Point> p = t.aStar(t.getTile(start),t.getTile(goal));
		for(int i = 0 ; i < p.size() ; i++){
			System.out.println((i+1)+" -> "+p.get(i).getX()+","+p.get(i).getY()+" - tile = "+t.getTile(p.get(i)));
		} System.out.println();
		
	}
	*/
	
	/*
	public static void main(String[] args) {
		
		Tiles tiles = new Tiles(300,300,5);
		*/
		/* Testando getNeighbors
		for(int i = 0 ; i < tiles.getTotalTiles() ; i++){
			Vector<Integer> neig = tiles.getNeighborsTiles(i);
			for(int j = 0 ; j < neig.size() ; j++){
				System.out.print(neig.get(j)+" ");
			} System.out.println();
		}
		*/
		
		/* Testando getTile 
		System.out.println(tiles.getTile(new Point(110,70)));
		System.out.println(tiles.getTile(new Point(210,170)));
		System.out.println(tiles.getTile(new Point(300,300)));
		*/
		
		/* Testando getCentralPoint 
		System.out.println(tiles.getCentral(6).getX()+","+tiles.getCentral(6).getY());
		System.out.println(tiles.getCentral(13).getX()+","+tiles.getCentral(13).getY());
		
		System.out.println(tiles.getCentral(new Point(110,70)).getX()+","+tiles.getCentral(new Point(110,70)).getY());
		System.out.println(tiles.getCentral(new Point(210,170)).getX()+","+tiles.getCentral(new Point(210,170)).getY());
		
		System.out.println(tiles.getCentral(new Point(0,0)).getX()+","+tiles.getCentral(new Point(0,0)).getY());
		System.out.println(tiles.getCentral(new Point(300,300)).getX()+","+tiles.getCentral(new Point(300,300)).getY());
		System.out.println(tiles.getCentral(new Point(30,30)).getX()+","+tiles.getCentral(new Point(30,30)).getY());
		System.out.println(tiles.getCentral(new Point(1,1)).getX()+","+tiles.getCentral(new Point(1,1)).getY());
		*/
		/*
	}*/
	
}
